from functools import lru_cache
import sys

from scipy.optimize import minimize

from tma.utils import settings as s
from tma.utils.custom_types import t_line, t_lines, t_point

from tma.utils.tools import *


def find_points_on_line_by_distance(p0: t_point, line: t_line, distance: float | int) -> tuple[bool, t_point, t_point]:
    """
    give a point p, a line l and a value v
    d =  perpendicular distance of p to l
    if d < v, return p1 and p2, where p1->p p2->p == d
    """
    x0, y0 = p0
    x1, y1, x2, y2 = line

    d_p2l = cal_perpendicular_distance(p0, line)
    if d_p2l > distance:
        return False, (0, 0), (0, 0)

    if x2 == x1:
        # print('x2 = = x1')
        dy = m.sqrt(distance**2 - (x0 - x1) ** 2)
        return True, (x1, y0 + dy), (x1, y0 - dy)

    else:
        x0, y0 = find_vertical_point(line, p0)
        d_shift = m.sqrt(distance**2 - d_p2l**2)
        slope, _ = get_line_slope_and_intercept(line)

        # dx ** 2 + (slope * dx) ** 2 = d_shift ** 2
        # dx ** 2 =  d_shift ** 2 / (1 + slope ** 2)
        dx = m.sqrt(d_shift**2 / (1 + slope**2))

        x1 = x0 + dx
        y1 = calculate_y_coordinate(line, x1)
        x2 = x0 - dx
        y2 = calculate_y_coordinate(line, x2)

        assert distance - distance_between_points(p0, [x1, y1]) < 1
        assert distance - distance_between_points(p0, [x2, y2]) < 1
        return True, (x1, y1), (x2, y2)


def evaluate_track_06(track_distance: float, bl: t_lines, cross_line: t_line, bl_time_tick: list[int]) -> float:
    """give track distance, and bearing lines info, calculate track error uing minimize()"""

    outer_lines_cross_p = find_intersection_point(bl[0], bl[-1])
    if not outer_lines_cross_p:
        raise Exception("First and last bearring seems parallel")
    try_point = outer_lines_cross_p
    # point on x-axis where line pass it and view_center is vertical to first_line
    x_value, _ = find_intersection_point(cross_line, bl[0])

    if x_value < outer_lines_cross_p[0]:
        try_step = -s.Try_point_step
    elif x_value > outer_lines_cross_p[0]:
        try_step = s.Try_point_step
    else:
        # @TODO first_line is vertical to x-axis
        raise Exception("predict_target_track error: first_line is vertical to x-axis")

    smallest_error = 99999
    while cal_perpendicular_distance(try_point, bl[-1]) <= track_distance:
        try_point = (
            try_point[0] + try_step,
            calculate_y_coordinate(bl[0], try_point[0] + try_step),
        )
        ok, p1, p2 = find_points_on_line_by_distance(try_point, bl[-1], track_distance)
        if not ok:
            continue

        p1_error = _cached_cal_track_fitness_metric(try_point, p1, bl, bl_time_tick)
        p2_error = _cached_cal_track_fitness_metric(try_point, p2, bl, bl_time_tick)

        smallest_error = min(p1_error, p2_error, smallest_error)
    return smallest_error


def _cached_cal_track_fitness_metric(
    head_point: t_point,
    tail_point: t_point,
    bl: t_lines,
    bl_time_tick: list[int],
) -> float:
    """calculate track fit metric using postion-diff method"""
    assume_track_line = list(head_point) + list(tail_point)
    error_list = []
    points = divide_track_by_bl(assume_track_line, bl_time_tick)
    assert len(points) == len(bl)
    for point, line in zip(points, bl):
        error_list.append(distance_between_points(point, find_intersection_point(assume_track_line, line)))
    y_true = [0 for _ in range(len(bl))]
    y_pred = error_list
    distance = distance_between_points(head_point, tail_point)
    return sum((yt - yp) ** 2 for yt, yp in zip(y_true, y_pred)) / (len(y_true) * distance)


def cal_track_error_06(
    track_distance: float,
    bl: t_lines,
    cross_line: t_line,
    bl_time_tick: list[int],
) -> float:
    return evaluate_track_06(track_distance, bl, cross_line, bl_time_tick)


def predict_06(bounds: list[tuple], bl: t_lines, cross_line: t_line, bl_time_tick: list[int]):
    # bounds = [(min(try_x_bottom, try_x_top), max(try_x_bottom, try_x_top)), (None, None)]
    res = minimize(cal_track_error_06, 200, args=(bl, cross_line, bl_time_tick), bounds=bounds)
    # return res.success, res.x
    # return res.success, res.status, res.nit, res.message, res.x
    print(res.success, res.status, res.nit, res.message, res.x)
